#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from std_msgs.msg import Int32MultiArray

def calculate_output(x):
    # 使用给定公式进行计算
    y = 0 + 2.315 * x + 0.1007 * x**2 - 0.03878 * x**3
    # 与 0.1 进行比较，返回 1 或 0
    return 1 if y > 0.1 else 0

def speed_callback(msg):
    # 对接收到的每个速度值进行计算和判断
    results = [calculate_output(x) for x in msg.data]
    # 打印计算结果
    rospy.loginfo("Processed output values: %s", ', '.join(map(str, results)))

def listener():
    # 初始化 ROS 节点
    rospy.init_node('speed_listener', anonymous=True)
    
    # 订阅 "/speed_values" 话题
    rospy.Subscriber("/speed_values", Int32MultiArray, speed_callback)
    
    # 保持节点运行，直到被手动关闭
    rospy.spin()

if __name__ == '__main__':
    try:
        listener()
    except rospy.ROSInterruptException:
        pass